package com.example.testdemo.ai;

import java.util.ArrayList;
import java.util.List;

import org.andengine.entity.modifier.PathModifier.Path;
import org.andengine.entity.sprite.Sprite;
import org.andengine.extension.tmx.TMXLayer;
import org.andengine.extension.tmx.TMXTile;
import org.andengine.extension.tmx.TMXTiledMap;

import com.example.testdemo.manager.ResourcesManager;
import com.example.testdemo.manager.ScenesManager;
import com.example.testdemo.resources.stage.BaseStageResources;

public class AStarSearch {

	@SuppressWarnings("unused")
    private static final String TAG = AStarSearch.class.getSimpleName();

	static class GridLoc {
		public float g = 0.0f; // dx from start (measured)
		public float h = 0.0f; // dx to target (heuristic)
		public boolean obstacle = false;
		public boolean wall = false;
		public boolean footprint = false;
	}

	static class AStar {
		private GridLoc[][] grid;
		private int rowMax, colMax;
		private int cellWidth, cellHeight;

		public AStar(int pRows, int pCols, int pTileWidth, int pTileHeight) {
			// pWidth = total width in pixels
			// pHeight = total height in pixels
			grid = new GridLoc[pRows][pCols];
			rowMax = pRows - 1;
			colMax = pCols - 1;
			cellWidth = pTileWidth;
			cellHeight = pTileHeight;
			for (int i = 0; i < pRows; i++) {
				for (int j = 0; j < pCols; j++) {
					grid[i][j] = new GridLoc();
				}
			}
		}

		private void setWall(int row, int col) {
			if (grid != null) {
				grid[row][col].wall = true;
			}
		}

		private void setObstacle(int row, int col) {
			if (grid != null) {
				grid[row][col].obstacle = true;
			}
		}

		private float fComp(int pCurrRow, int pCurrCol, int pRowDiff,
				int pColDiff, float pDx, int pTargetRow, int pTargetCol) {
			// Computes the A* values for a grid location:
			// g: distance from start
			// h: distance to target
			// returns f = g + h
			// If the grid location is marked as an obstacle,
			// footprint is true; if it is outside the grid,
			// returns a high number (5,000)
			if (((pCurrRow + pRowDiff) > rowMax)
					|| ((pCurrCol + pColDiff) > colMax)
					|| ((pCurrRow + pRowDiff) < 0)
					|| ((pCurrCol + pColDiff) < 0)) {
				return 5000.0f;
			}
			if ((grid[pCurrRow + pRowDiff][pCurrCol + pColDiff].obstacle)
					|| (grid[pCurrRow + pRowDiff][pCurrCol + pColDiff].wall)
					|| (grid[pCurrRow + pRowDiff][pCurrCol + pColDiff].footprint)) {
				return 5000.0f;
			}
			grid[pCurrRow + pRowDiff][pCurrCol + pColDiff].g = grid[pCurrRow][pCurrCol].g
					+ pDx;
			grid[pCurrRow + pRowDiff][pCurrCol + pColDiff].h = Math
					.abs(pTargetRow - (pCurrRow + pRowDiff))
					+ Math.abs(pTargetCol - (pCurrCol + pColDiff));
			return (grid[pCurrRow + pRowDiff][pCurrCol + pColDiff].g + grid[pCurrRow
					+ pRowDiff][pCurrCol + pColDiff].h);
		}

		private Path generatePath4Directions(float pStartX, float pStartY,
				int pTargetRow, int pTargetCol) throws Exception {
			int nextCol, nextRow;
			int startCol, startRow;
			ArrayList<Integer> pathCols = new ArrayList<Integer>();
			ArrayList<Integer> pathRows = new ArrayList<Integer>();
			startCol = (int) pStartX / cellWidth;
			startRow = (int) pStartY / cellHeight;
			int currCol = startCol;
			int currRow = startRow;
			float[] f = new float[4];
			grid[currRow][currCol].g = 0.0f;
			grid[currRow][currCol].h = pTargetCol - currCol + pTargetRow
					- currRow;
			grid[currRow][currCol].footprint = true;
			while ((currCol != pTargetCol) || (currRow != pTargetRow)) {
				// Consider the eight surrounding locations
				for (int i = 0; i < 4; i++)
					f[i] = 0;
				f[0] = fComp(currRow, currCol, 0, -1, 1.0f, pTargetRow,
						pTargetCol);
				f[1] = fComp(currRow, currCol, -1, 0, 1.0f, pTargetRow,
						pTargetCol);
				f[2] = fComp(currRow, currCol, +1, 0, 1.0f, pTargetRow,
						pTargetCol);
				f[3] = fComp(currRow, currCol, 0, +1, 1.0f, pTargetRow,
						pTargetCol);
				int lowidx = 0;
				float pos = 10000.0f;
				for (int j = 0; j < 4; j++) {
					if (f[j] < pos) {
						pos = f[j];
						lowidx = j;
					}
				}
				nextCol = currCol;
				nextRow = currRow;
				switch (lowidx) {
				case (0):
					nextRow = currRow;
					nextCol = currCol - 1;
					break;
				case (1):
					nextRow = currRow - 1;
					nextCol = currCol;
					break;
				case (2):
					nextRow = currRow + 1;
					nextCol = currCol;
					break;
				case (3):
					nextRow = currRow;
					nextCol = currCol + 1;
					break;
				}
				// Add next location to Path,
				// set footprint and update currCol, currRow
				pathCols.add(nextCol);
				pathRows.add(nextRow);
				grid[currRow][currCol].footprint = true;
				currCol = nextCol;
				currRow = nextRow;
			}
			float[] xArray = new float[pathCols.size() + 1];
			float[] yArray = new float[pathRows.size() + 1];
			xArray[0] = pStartX;
			yArray[0] = pStartY;
			for (int i = 1; i < xArray.length; i++) {
				Float tmpX = (float) pathCols.get(i - 1) * cellWidth;
				xArray[i] = (tmpX != null ? tmpX : 0.0f);
				Float tmpY = (float) pathRows.get(i - 1) * cellHeight;
				yArray[i] = (tmpY != null ? tmpY : 0.0f);
			}
			return (new Path(xArray, yArray));
		}

		@SuppressWarnings("unused")
        private Path generatePath8Directions(float pStartX, float pStartY,
				int pTargetRow, int pTargetCol) throws Exception {
			// Use A* pathfinding to find the near optimal path
			int nextCol, nextRow;
			int startCol, startRow;
			ArrayList<Integer> pathCols = new ArrayList<Integer>();
			ArrayList<Integer> pathRows = new ArrayList<Integer>();
			startCol = (int) pStartX / cellWidth;
			startRow = (int) pStartY / cellHeight;
			int currCol = startCol;
			int currRow = startRow;
			float[] f = new float[8];
			grid[currRow][currCol].g = 0.0f;
			grid[currRow][currCol].h = pTargetCol - currCol + pTargetRow
					- currRow;
			grid[currRow][currCol].footprint = true;
			while ((currCol != pTargetCol) || (currRow != pTargetRow)) {
				// Consider the eight surrounding locations
				for (int i = 0; i < 8; i++)
					f[i] = 0;
				f[0] = fComp(currRow, currCol, -1, -1, 1.4f, pTargetRow,
						pTargetCol);
				f[1] = fComp(currRow, currCol, 0, -1, 1.0f, pTargetRow,
						pTargetCol);
				f[2] = fComp(currRow, currCol, +1, -1, 1.4f, pTargetRow,
						pTargetCol);
				f[3] = fComp(currRow, currCol, -1, 0, 1.0f, pTargetRow,
						pTargetCol);
				f[4] = fComp(currRow, currCol, +1, 0, 1.0f, pTargetRow,
						pTargetCol);
				f[5] = fComp(currRow, currCol, -1, +1, 1.4f, pTargetRow,
						pTargetCol);
				f[6] = fComp(currRow, currCol, 0, +1, 1.0f, pTargetRow,
						pTargetCol);
				f[7] = fComp(currRow, currCol, +1, +1, 1.4f, pTargetRow,
						pTargetCol);
				int lowidx = 0;
				float pos = 10000.0f;
				for (int j = 0; j < 8; j++) {
					if (f[j] < pos) {
						pos = f[j];
						lowidx = j;
					}
				}
				nextCol = currCol;
				nextRow = currRow;
				switch (lowidx) {
				case (0):
					nextRow = currRow - 1;
					nextCol = currCol - 1;
					break;
				case (1):
					nextRow = currRow;
					nextCol = currCol - 1;
					break;
				case (2):
					nextRow = currRow + 1;
					nextCol = currCol - 1;
					break;
				case (3):
					nextRow = currRow - 1;
					nextCol = currCol;
					break;
				case (4):
					nextRow = currRow + 1;
					nextCol = currCol;
					break;
				case (5):
					nextRow = currRow - 1;
					nextCol = currCol + 1;
					break;
				case (6):
					nextRow = currRow;
					nextCol = currCol + 1;
					break;
				case (7):
					nextRow = currRow + 1;
					nextCol = currCol + 1;
					break;
				}
				// Add next location to Path,
				// set footprint and update currCol, currRow
				pathCols.add(nextCol);
				pathRows.add(nextRow);
				grid[currRow][currCol].footprint = true;
				currCol = nextCol;
				currRow = nextRow;
			}
			float[] xArray = new float[pathCols.size() + 1];
			float[] yArray = new float[pathRows.size() + 1];
			xArray[0] = pStartX;
			yArray[0] = pStartY;
			for (int i = 1; i < xArray.length; i++) {
				Float tmpX = (float) pathCols.get(i - 1) * cellWidth;
				xArray[i] = (tmpX != null ? tmpX : 0.0f);
				Float tmpY = (float) pathRows.get(i - 1) * cellHeight;
				yArray[i] = (tmpY != null ? tmpY : 0.0f);
			}
			return (new Path(xArray, yArray));
		}

	}

	public static Path search(final Sprite start, final TMXTile toTile,
			List<Sprite> wallList, List<Sprite> obstacleList) {
		final BaseStageResources resources = (BaseStageResources) ResourcesManager
				.getInstance().getResources(ScenesManager.SCENE_STAGE_1);

		final TMXTiledMap map = resources.mMap;
		final TMXLayer layer = map.getTMXLayers().get(0);

		AStar aStar = new AStar(map.getTileRows(), map.getTileColumns(),
				map.getTileWidth(), map.getTileHeight());

		// Set obstacle
		for (Sprite obstacle : obstacleList) {
			TMXTile tile = map.getTMXLayers().get(0)
					.getTMXTileAt(obstacle.getX(), obstacle.getY());
			aStar.setObstacle(tile.getTileRow(), tile.getTileColumn());
		}

		// Set wall
		for (Sprite wall : wallList) {
			TMXTile tile = map.getTMXLayers().get(0)
					.getTMXTileAt(wall.getX(), wall.getY());
			aStar.setWall(tile.getTileRow(), tile.getTileColumn());
		}

		// Set start & to point
		TMXTile startTile = layer.getTMXTileAt(start.getX() + start.getWidth()
				/ 2, start.getY() + start.getHeight() / 2);

		// if (DEBUG_TILES) {
		// if (targetTile != null) {
		// Rectangle debugRectangle = new Rectangle(0, 0,
		// map.getTileWidth(), map.getTileHeight(),
		// activity.getVertexBufferObjectManager());
		// debugRectangle.setColor(1, 0, 0, 0.25f);
		// demoScene.attachChild(debugRectangle);
		// debugRectangle.setPosition(targetTile.getTileX(),
		// targetTile.getTileY());
		// }
		// }

		try {
			return aStar.generatePath4Directions(startTile.getTileX(),
					startTile.getTileY(), toTile.getTileRow(),
					toTile.getTileColumn());
		} catch (Exception e) {
			// String debug = String.format(
			// "from startTile(%d, %d) to toTile(%d, %d)",
			// startTile.getTileRow(), startTile.getTileColumn(),
			// toTile.getTileRow(), toTile.getTileColumn());
			// Log.d(TAG, "Can't find path " + debug);
//
//			// Find 5 nearest wall according Manhattan distance
//			Map<TMXTile, Integer> nearestMap = new HashMap<TMXTile, Integer>();
//			for (Sprite wall : wallList) {
//				TMXTile tile = map.getTMXLayers().get(0)
//						.getTMXTileAt(wall.getX(), wall.getY());
//				int distance = Math.abs(startTile.getTileRow()
//						- tile.getTileRow())
//						+ Math.abs(startTile.getTileColumn()
//								- tile.getTileColumn());
//
//				if (nearestMap.size() < 5) {
//					nearestMap.put(tile, distance);
//					continue;
//				}
//
//				Iterator<TMXTile> iterator = nearestMap.keySet().iterator();
//				while (iterator.hasNext()) {
//					TMXTile minTile = iterator.next();
//					if (distance < nearestMap.get(minTile)) {
//						nearestMap.remove(minTile);
//						nearestMap.put(tile, distance);
//						break;
//					}
//				}
//			}
//
//			// Generate a path to a reachable nearest wall
//			for (TMXTile wall : nearestMap.keySet()) {
//				try {
//					return aStar.generatePath4Directions(startTile.getTileX(),
//							startTile.getTileY(), wall.getTileRow() - 1,
//							wall.getTileColumn() - 1);
//				} catch (Exception e1) {
//					continue;
//				}
//			}

		}

//		Log.d(TAG, "There is no reachable path, standby!");
		return null;
	}
}
